Path Planning for Multiple Features based Localization

In surveillance or exploration mission in a known environment, the localization of the dedicated sensor is of main importance. In this paper, we discuss the path planning problem for the localization algorithm which correlates range and bearing measurements and a map composed of several features. The sensor motion is designed from an information measure derived from the Fisher Information Matrix. It is shown that a closed form expression of the cost can be obtained. The optimal features location can be neatly geometrically interpreted. An integral cost which includes the sensor perception limitation is then formulated. It is used in a dynamic programming framework to tackle the path optimization problem.